1   package org.apache.lucene.util;
2   
3   /*
4    * Licensed to the Apache Software Foundation (ASF) under one or more
5    * contributor license agreements.  See the NOTICE file distributed with
6    * this work for additional information regarding copyright ownership.
7    * The ASF licenses this file to You under the Apache License, Version 2.0
8    * (the "License"); you may not use this file except in compliance with
9    * the License.  You may obtain a copy of the License at
10   *
11   *     http://www.apache.org/licenses/LICENSE-2.0
12   *
13   * Unless required by applicable law or agreed to in writing, software
14   * distributed under the License is distributed on an "AS IS" BASIS,
15   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16   * See the License for the specific language governing permissions and
17   * limitations under the License.
18   */
19  
20  /**
21   * Reusable geo-spatial projection utility methods.
22   *
23   * @lucene.experimental
24   */
25  public class GeoProjectionUtils {
26    // WGS84 earth-ellipsoid major (a) minor (b) radius, (f) flattening and eccentricity (e)
27    public static final double SEMIMAJOR_AXIS = 6_378_137; // [m]
28    public static final double FLATTENING = 1.0/298.257223563;
29    public static final double SEMIMINOR_AXIS = SEMIMAJOR_AXIS * (1.0 - FLATTENING); //6_356_752.31420; // [m]
30    public static final double ECCENTRICITY = StrictMath.sqrt((2.0 - FLATTENING) * FLATTENING);
31    static final double PI_OVER_2 = StrictMath.PI / 2.0D;
32    static final double SEMIMAJOR_AXIS2 = SEMIMAJOR_AXIS * SEMIMAJOR_AXIS;
33    static final double SEMIMINOR_AXIS2 = SEMIMINOR_AXIS * SEMIMINOR_AXIS;
34    public static final double MIN_LON_RADIANS = StrictMath.toRadians(GeoUtils.MIN_LON_INCL);
35    public static final double MIN_LAT_RADIANS = StrictMath.toRadians(GeoUtils.MIN_LAT_INCL);
36    public static final double MAX_LON_RADIANS = StrictMath.toRadians(GeoUtils.MAX_LON_INCL);
37    public static final double MAX_LAT_RADIANS = StrictMath.toRadians(GeoUtils.MAX_LAT_INCL);
38  
39    /**
40     * Converts from geocentric earth-centered earth-fixed to geodesic lat/lon/alt
41     * @param x Cartesian x coordinate
42     * @param y Cartesian y coordinate
43     * @param z Cartesian z coordinate
44     * @param lla 0: longitude 1: latitude: 2: altitude
45     * @return double array as 0: longitude 1: latitude 2: altitude
46     */
47    public static final double[] ecfToLLA(final double x, final double y, final double z, double[] lla) {
48      boolean atPole = false;
49      final double ad_c = 1.0026000D;
50      final double e2 = (SEMIMAJOR_AXIS2 - SEMIMINOR_AXIS2)/(SEMIMAJOR_AXIS2);
51      final double ep2 = (SEMIMAJOR_AXIS2 - SEMIMINOR_AXIS2)/(SEMIMINOR_AXIS2);
52      final double cos67P5 = 0.38268343236508977D;
53  
54      if (lla == null) {
55        lla = new double[3];
56      }
57  
58      if (x != 0.0) {
59        lla[0] = StrictMath.atan2(y,x);
60      } else {
61        if (y > 0) {
62          lla[0] = PI_OVER_2;
63        } else if (y < 0) {
64          lla[0] = -PI_OVER_2;
65        } else {
66          atPole = true;
67          lla[0] = 0.0D;
68          if (z > 0.0) {
69            lla[1] = PI_OVER_2;
70          } else if (z < 0.0) {
71            lla[1] = -PI_OVER_2;
72          } else {
73            lla[1] = PI_OVER_2;
74            lla[2] = -SEMIMINOR_AXIS;
75            return lla;
76          }
77        }
78      }
79  
80      final double w2 = x*x + y*y;
81      final double w = StrictMath.sqrt(w2);
82      final double t0 = z * ad_c;
83      final double s0 = StrictMath.sqrt(t0 * t0 + w2);
84      final double sinB0 = t0 / s0;
85      final double cosB0 = w / s0;
86      final double sin3B0 = sinB0 * sinB0 * sinB0;
87      final double t1 = z + SEMIMINOR_AXIS * ep2 * sin3B0;
88      final double sum = w - SEMIMAJOR_AXIS * e2 * cosB0 * cosB0 * cosB0;
89      final double s1 = StrictMath.sqrt(t1 * t1 + sum * sum);
90      final double sinP1 = t1 / s1;
91      final double cosP1 = sum / s1;
92      final double rn = SEMIMAJOR_AXIS / StrictMath.sqrt(1.0D - e2 * sinP1 * sinP1);
93  
94      if (cosP1 >= cos67P5) {
95        lla[2] = w / cosP1 - rn;
96      } else if (cosP1 <= -cos67P5) {
97        lla[2] = w / -cosP1 - rn;
98      } else {
99        lla[2] = z / sinP1 + rn * (e2 - 1.0);
100     }
101     if (!atPole) {
102       lla[1] = StrictMath.atan(sinP1/cosP1);
103     }
104     lla[0] = StrictMath.toDegrees(lla[0]);
105     lla[1] = StrictMath.toDegrees(lla[1]);
106 
107     return lla;
108   }
109 
110   /**
111    * Converts from geodesic lon lat alt to geocentric earth-centered earth-fixed
112    * @param lon geodesic longitude
113    * @param lat geodesic latitude
114    * @param alt geodesic altitude
115    * @param ecf reusable earth-centered earth-fixed result
116    * @return either a new ecef array or the reusable ecf parameter
117    */
118   public static final double[] llaToECF(double lon, double lat, double alt, double[] ecf) {
119     lon = StrictMath.toRadians(lon);
120     lat = StrictMath.toRadians(lat);
121 
122     final double sl = StrictMath.sin(lat);
123     final double s2 = sl*sl;
124     final double cl = StrictMath.cos(lat);
125     final double ge2 = (SEMIMAJOR_AXIS2 - SEMIMINOR_AXIS2)/(SEMIMAJOR_AXIS2);
126 
127     if (ecf == null) {
128       ecf = new double[3];
129     }
130 
131     if (lat < -PI_OVER_2 && lat > -1.001D * PI_OVER_2) {
132       lat = -PI_OVER_2;
133     } else if (lat > PI_OVER_2 && lat < 1.001D * PI_OVER_2) {
134       lat = PI_OVER_2;
135     }
136     assert (lat >= -PI_OVER_2) || (lat <= PI_OVER_2);
137 
138     if (lon > StrictMath.PI) {
139       lon -= (2*StrictMath.PI);
140     }
141 
142     final double rn = SEMIMAJOR_AXIS / StrictMath.sqrt(1.0D - ge2 * s2);
143     ecf[0] = (rn+alt) * cl * StrictMath.cos(lon);
144     ecf[1] = (rn+alt) * cl * StrictMath.sin(lon);
145     ecf[2] = ((rn*(1.0-ge2))+alt)*sl;
146 
147     return ecf;
148   }
149 
150   /**
151    * Converts from lat lon alt (in degrees) to East North Up right-hand coordinate system
152    * @param lon longitude in degrees
153    * @param lat latitude in degrees
154    * @param alt altitude in meters
155    * @param centerLon reference point longitude in degrees
156    * @param centerLat reference point latitude in degrees
157    * @param centerAlt reference point altitude in meters
158    * @param enu result east, north, up coordinate
159    * @return east, north, up coordinate
160    */
161   public static double[] llaToENU(final double lon, final double lat, final double alt, double centerLon,
162                                   double centerLat, final double centerAlt, double[] enu) {
163     if (enu == null) {
164       enu = new double[3];
165     }
166 
167     // convert point to ecf coordinates
168     final double[] ecf = llaToECF(lon, lat, alt, null);
169 
170     // convert from ecf to enu
171     return ecfToENU(ecf[0], ecf[1], ecf[2], centerLon, centerLat, centerAlt, enu);
172   }
173 
174   /**
175    * Converts from East North Up right-hand rule to lat lon alt in degrees
176    * @param x easting (in meters)
177    * @param y northing (in meters)
178    * @param z up (in meters)
179    * @param centerLon reference point longitude (in degrees)
180    * @param centerLat reference point latitude (in degrees)
181    * @param centerAlt reference point altitude (in meters)
182    * @param lla resulting lat, lon, alt point (in degrees)
183    * @return lat, lon, alt point (in degrees)
184    */
185   public static double[] enuToLLA(final double x, final double y, final double z, final double centerLon,
186                                   final double centerLat, final double centerAlt, double[] lla) {
187     // convert enuToECF
188     if (lla == null) {
189       lla = new double[3];
190     }
191 
192     // convert enuToECF, storing intermediate result in lla
193     lla = enuToECF(x, y, z, centerLon, centerLat, centerAlt, lla);
194 
195     // convert ecf to LLA
196     return ecfToLLA(lla[0], lla[1], lla[2], lla);
197   }
198 
199   /**
200    * Convert from Earth-Centered-Fixed to Easting, Northing, Up Right Hand System
201    * @param x ECF X coordinate (in meters)
202    * @param y ECF Y coordinate (in meters)
203    * @param z ECF Z coordinate (in meters)
204    * @param centerLon ENU origin longitude (in degrees)
205    * @param centerLat ENU origin latitude (in degrees)
206    * @param centerAlt ENU altitude (in meters)
207    * @param enu reusable enu result
208    * @return Easting, Northing, Up coordinate
209    */
210   public static double[] ecfToENU(double x, double y, double z, final double centerLon,
211                                   final double centerLat, final double centerAlt, double[] enu) {
212     if (enu == null) {
213       enu = new double[3];
214     }
215 
216     // create rotation matrix and rotate to enu orientation
217     final double[][] phi = createPhiTransform(centerLon, centerLat, null);
218 
219     // convert origin to ENU
220     final double[] originECF = llaToECF(centerLon, centerLat, centerAlt, null);
221     final double[] originENU = new double[3];
222     originENU[0] = ((phi[0][0] * originECF[0]) + (phi[0][1] * originECF[1]) + (phi[0][2] * originECF[2]));
223     originENU[1] = ((phi[1][0] * originECF[0]) + (phi[1][1] * originECF[1]) + (phi[1][2] * originECF[2]));
224     originENU[2] = ((phi[2][0] * originECF[0]) + (phi[2][1] * originECF[1]) + (phi[2][2] * originECF[2]));
225 
226     // rotate then translate
227     enu[0] = ((phi[0][0] * x) + (phi[0][1] * y) + (phi[0][2] * z)) - originENU[0];
228     enu[1] = ((phi[1][0] * x) + (phi[1][1] * y) + (phi[1][2] * z)) - originENU[1];
229     enu[2] = ((phi[2][0] * x) + (phi[2][1] * y) + (phi[2][2] * z)) - originENU[2];
230 
231     return enu;
232   }
233 
234   /**
235    * Convert from Easting, Northing, Up Right-Handed system to Earth Centered Fixed system
236    * @param x ENU x coordinate (in meters)
237    * @param y ENU y coordinate (in meters)
238    * @param z ENU z coordinate (in meters)
239    * @param centerLon ENU origin longitude (in degrees)
240    * @param centerLat ENU origin latitude (in degrees)
241    * @param centerAlt ENU origin altitude (in meters)
242    * @param ecf reusable ecf result
243    * @return ecf result coordinate
244    */
245   public static double[] enuToECF(final double x, final double y, final double z, double centerLon,
246                                   double centerLat, final double centerAlt, double[] ecf) {
247     if (ecf == null) {
248       ecf = new double[3];
249     }
250 
251     double[][] phi = createTransposedPhiTransform(centerLon, centerLat, null);
252     double[] ecfOrigin = llaToECF(centerLon, centerLat, centerAlt, null);
253 
254     // rotate and translate
255     ecf[0] = (phi[0][0]*x + phi[0][1]*y + phi[0][2]*z) + ecfOrigin[0];
256     ecf[1] = (phi[1][0]*x + phi[1][1]*y + phi[1][2]*z) + ecfOrigin[1];
257     ecf[2] = (phi[2][0]*x + phi[2][1]*y + phi[2][2]*z) + ecfOrigin[2];
258 
259     return ecf;
260   }
261 
262   /**
263    * Create the rotation matrix for converting Earth Centered Fixed to Easting Northing Up
264    * @param originLon ENU origin longitude (in degrees)
265    * @param originLat ENU origin latitude (in degrees)
266    * @param phiMatrix reusable phi matrix result
267    * @return phi rotation matrix
268    */
269   private static double[][] createPhiTransform(double originLon, double originLat, double[][] phiMatrix) {
270 
271     if (phiMatrix == null) {
272       phiMatrix = new double[3][3];
273     }
274 
275     originLon = StrictMath.toRadians(originLon);
276     originLat = StrictMath.toRadians(originLat);
277 
278     final double sLon = StrictMath.sin(originLon);
279     final double cLon = StrictMath.cos(originLon);
280     final double sLat = StrictMath.sin(originLat);
281     final double cLat = StrictMath.cos(originLat);
282 
283     phiMatrix[0][0] = -sLon;
284     phiMatrix[0][1] = cLon;
285     phiMatrix[0][2] = 0.0D;
286     phiMatrix[1][0] = -sLat * cLon;
287     phiMatrix[1][1] = -sLat * sLon;
288     phiMatrix[1][2] = cLat;
289     phiMatrix[2][0] = cLat * cLon;
290     phiMatrix[2][1] = cLat * sLon;
291     phiMatrix[2][2] = sLat;
292 
293     return phiMatrix;
294   }
295 
296   /**
297    * Create the transposed rotation matrix for converting Easting Northing Up coordinates to Earth Centered Fixed
298    * @param originLon ENU origin longitude (in degrees)
299    * @param originLat ENU origin latitude (in degrees)
300    * @param phiMatrix reusable phi rotation matrix result
301    * @return transposed phi rotation matrix
302    */
303   private static double[][] createTransposedPhiTransform(double originLon, double originLat, double[][] phiMatrix) {
304 
305     if (phiMatrix == null) {
306       phiMatrix = new double[3][3];
307     }
308 
309     originLon = StrictMath.toRadians(originLon);
310     originLat = StrictMath.toRadians(originLat);
311 
312     final double sLat = StrictMath.sin(originLat);
313     final double cLat = StrictMath.cos(originLat);
314     final double sLon = StrictMath.sin(originLon);
315     final double cLon = StrictMath.cos(originLon);
316 
317     phiMatrix[0][0] = -sLon;
318     phiMatrix[1][0] = cLon;
319     phiMatrix[2][0] = 0.0D;
320     phiMatrix[0][1] = -sLat * cLon;
321     phiMatrix[1][1] = -sLat * sLon;
322     phiMatrix[2][1] = cLat;
323     phiMatrix[0][2] = cLat * cLon;
324     phiMatrix[1][2] = cLat * sLon;
325     phiMatrix[2][2] = sLat;
326 
327     return phiMatrix;
328   }
329 
330   /**
331    * Finds a point along a bearing from a given lon,lat geolocation using vincenty's distance formula
332    *
333    * @param lon origin longitude in degrees
334    * @param lat origin latitude in degrees
335    * @param bearing azimuthal bearing in degrees
336    * @param dist distance in meters
337    * @param pt resulting point
338    * @return the point along a bearing at a given distance in meters
339    */
340   public static final double[] pointFromLonLatBearing(double lon, double lat, double bearing, double dist, double[] pt) {
341 
342     if (pt == null) {
343       pt = new double[2];
344     }
345 
346     final double alpha1 = StrictMath.toRadians(bearing);
347     final double cosA1 = StrictMath.cos(alpha1);
348     final double sinA1 = StrictMath.sin(alpha1);
349     final double tanU1 = (1-FLATTENING) * StrictMath.tan(StrictMath.toRadians(lat));
350     final double cosU1 = 1 / StrictMath.sqrt((1+tanU1*tanU1));
351     final double sinU1 = tanU1*cosU1;
352     final double sig1 = StrictMath.atan2(tanU1, cosA1);
353     final double sinAlpha = cosU1 * sinA1;
354     final double cosSqAlpha = 1 - sinAlpha*sinAlpha;
355     final double uSq = cosSqAlpha * (SEMIMAJOR_AXIS2 - SEMIMINOR_AXIS2) / SEMIMINOR_AXIS2;
356     final double A = 1 + uSq/16384D*(4096D + uSq * (-768D + uSq * (320D - 175D*uSq)));
357     final double B = uSq/1024D * (256D + uSq * (-128D + uSq * (74D - 47D * uSq)));
358 
359     double sigma = dist / (SEMIMINOR_AXIS*A);
360     double sigmaP;
361     double sinSigma, cosSigma, cos2SigmaM, deltaSigma;
362 
363     do {
364       cos2SigmaM = StrictMath.cos(2*sig1 + sigma);
365       sinSigma = StrictMath.sin(sigma);
366       cosSigma = StrictMath.cos(sigma);
367 
368       deltaSigma = B * sinSigma * (cos2SigmaM + (B/4D) * (cosSigma*(-1+2*cos2SigmaM*cos2SigmaM)-
369           (B/6) * cos2SigmaM*(-3+4*sinSigma*sinSigma)*(-3+4*cos2SigmaM*cos2SigmaM)));
370       sigmaP = sigma;
371       sigma = dist / (SEMIMINOR_AXIS*A) + deltaSigma;
372     } while (StrictMath.abs(sigma-sigmaP) > 1E-12);
373 
374     final double tmp = sinU1*sinSigma - cosU1*cosSigma*cosA1;
375     final double lat2 = StrictMath.atan2(sinU1*cosSigma + cosU1*sinSigma*cosA1,
376         (1-FLATTENING) * StrictMath.sqrt(sinAlpha*sinAlpha + tmp*tmp));
377     final double lambda = StrictMath.atan2(sinSigma*sinA1, cosU1*cosSigma - sinU1*sinSigma*cosA1);
378     final double c = FLATTENING/16 * cosSqAlpha * (4 + FLATTENING * (4 - 3 * cosSqAlpha));
379 
380     final double lam = lambda - (1-c) * FLATTENING * sinAlpha *
381         (sigma + c * sinSigma * (cos2SigmaM + c * cosSigma * (-1 + 2* cos2SigmaM*cos2SigmaM)));
382     pt[0] = GeoUtils.normalizeLon(lon + StrictMath.toDegrees(lam));
383     pt[1] = GeoUtils.normalizeLat(StrictMath.toDegrees(lat2));
384 
385     return pt;
386   }
387 }